/*************************************************************************
*
*	trajplan.c
*
*  Copyright 2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module converts incoming commands (waypoint, heading, station keep)
*    into rudder + throttle values.  
*
*
**************************************************************************
*
*  version 0.2.5, 4/10/09, SBS
*    - add throttle debug output for heading hold
*
*  version 0.2.4, 3/7/09, SBS
*    - add trajplan_Test()
*
*  version 0.2.3, 3/6/09, SBS
*    - changes to headinghold rudder heuristics
*
*  version 0.2.2, 2/2/09, SBS
*    - add _GetStatus
*
*  version 0.2.1, 2/1/09, SBS
*    - rename global vars, clean up mutex usage
*
*  version 0.2, 1/30/09, SBS
*    - remember old rudder and throttle values 
*    - add verbose output to skeletal waypoint function
*
*  version 0.1, 1/9/09, SBS
*    - first skeletal version
*    - implements a very dumb heading-keep mode
*    - other modes simply freedrift
*
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_engineering
*  @brief
*  This module converts incoming commands (waypoint, heading, station keep)
*    into rudder + throttle values.  
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <time.h>
#include <sys/timeb.h>
#include <pthread.h>

#include "types.h"

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"

#include "delay.h"

#ifdef DEBUG
#include "logfile.h"
#endif

#include "config.h"
#include "command.h"
#include "platformstatus.h"

#include "controls.h"
#include "trajplan.h"

//**************************************************************************

static BYTE global_rudder = 0;
static BYTE global_throttle = 0;

static pthread_mutex_t rudderthrottle_mutex = PTHREAD_MUTEX_INITIALIZER;


/**************************************************************************
*
* trajplan_GetStatus()
*/
/**
* @brief
* Retrieves the current rudder & throttle commanded values
*
*
**************************************************************************/
void trajplan_GetStatus(STATUS *s)
{
	pthread_mutex_lock( &rudderthrottle_mutex );
	s->dRudderCommand.dValue = global_rudder;
	s->dThrottleCommand.dValue = global_throttle;
	pthread_mutex_unlock( &rudderthrottle_mutex );
	
	return;
}


/**************************************************************************
*
* trajplan_StationKeep()
*/
/**
* @brief
* Determines rudder and throttle settings to achieve
*    station keeping near a given waypoint.
*
*
**************************************************************************/
/*void trajplan_Calibrate(void)	
{
	STATUS s;
	int r,t;

	s = platformstatus_GetStatus();
	r = 0;
	t = 0;
	
	controls_SetRudder(r);
	controls_SetThrottle(t);

	pthread_mutex_lock( &rudderthrottle_mutex );
	global_rudder = r;
	global_throttle = t;
	pthread_mutex_unlock( &rudderthrottle_mutex );
	
	return;
}
*/


/********************************************************
*
* trajplan_StationKeep()
*/
/**
* @brief
* Used for testing operation of PWM output subsystem
*
*
********************************************************/
static void trajplan_Test(void)
{
	static struct timeb oldtime;
	static int memory=0;

	struct timeb newtime;
	double delta;

	int r,t;	
	
	if(memory==0){
		ftime(&oldtime);	
		memory=1;
		r = 50;
		t = 50;
		
		controls_SetRudder(r);
		controls_SetThrottle(t);

		pthread_mutex_lock( &rudderthrottle_mutex );
		global_rudder = r;
		global_throttle = t;
		pthread_mutex_unlock( &rudderthrottle_mutex );

		return;
	}

	ftime(&newtime);	

	delta = delay_deltatime(oldtime,newtime);
		
	if(delta > (1.5)){
		
		pthread_mutex_lock( &rudderthrottle_mutex );
		global_rudder = -global_rudder;
		global_throttle = -global_throttle;
		r = global_rudder;
		t = global_throttle;
		pthread_mutex_unlock( &rudderthrottle_mutex );

		controls_SetRudder(r);
		controls_SetThrottle(t);

		ftime(&oldtime);
	}
	
	return;

}


/********************************************************
*
* trajplan_StationKeep()
*/
/**
* @brief
* Determines rudder and throttle settings to achieve
*    station keeping near a given waypoint.
*
*
********************************************************/
void trajplan_StationKeep(
		WAYPT wp)	
{
/*	STATUS s;
	int r,t;

	s = platformstatus_GetStatus();
	r = 0;
	t = 0;
	
	controls_SetRudder(r);
	controls_SetThrottle(t);

	pthread_mutex_lock( &rudderthrottle_mutex );
	global_rudder = r;
	global_throttle = t;
	pthread_mutex_unlock( &rudderthrottle_mutex );
*/
	
	trajplan_Test();

	return;
}


/********************************************************
*
* trajplan_HeadingHold()
*/
/**
* @brief
* Determines rudder and throttle settings to maintain
*    a given heading.
*
*
********************************************************/
void trajplan_HeadingHold(
		WORD heading
		,BYTE thr)	
{
	STATUS s;
	int delta;
	int r,t;
		
	// retrieve current boat telemetry
	s = platformstatus_GetStatus();
	
	// difference between current heading and goal
	delta = (int)(s.dHdgDeg - (double)heading);
	
	printf("============\n");
	printf("Heading hold\n");
	printf("goal = %d      throttle = %d\n"
			,heading,thr);
	printf("heading = %f   delta = %d\n"
			,s.dHdgDeg,delta);
			
	// adjust to be within -180:180 range
	if(delta > 180){
		delta -= 360;
	}else if(delta < -180){
		delta += 360;
	}

	printf("adjusted delta = %d\n",delta);

	if(delta>90){
		r = 80;
	}else if(delta>45){
		r = 60;
	}else if(delta>5){
		r = 30;
	}else if(delta<-90){
		r = -80;
	}else if(delta<-45){
		r = -60;
	}else if(delta <-5){
		r = -30;
	}else{
		r = 0;
	}
	
	printf("rudder output = %d\n",r);
		
	// divide by 2 gets it within -100:+100 range
//	r = delta / 2;
	
	// reduce speed if sharp steering angle
	if(abs(r) > 45){
		t = thr / 2;
	}else{
		t = thr;
	}

	#ifdef DEBUG
	{
	char logstr[LOGSTRLEN];
	snprintf(logstr,sizeof(logstr)
				,"heading = %f  goal = %d  delta=%d  rudder=%d\n"
				,s.dHdgDeg,heading,delta,r);
	logfile_LogLevelPrint(LOGLEVEL_DEBUG,logstr);
	}
	#endif
	
	controls_SetRudder(r);
	controls_SetThrottle(t);

	pthread_mutex_lock( &rudderthrottle_mutex );
	global_rudder = r;
	global_throttle = t;
	pthread_mutex_unlock( &rudderthrottle_mutex );
	
	return;
}

#define PRINTSCREEN
/******************************************************************************
*
* trajplan_WaypointAchieve()
*/
/**
* @brief
* Determines rudder and throttle settings to reach
*  a waypoint with a heading towards a second waypoint.
*
*
******************************************************************************/
void trajplan_WaypointAchieve(
		WAYPT wp0
		,WAYPT wp1)	
{
	STATUS s;
	int r,t;
	
	#ifdef DEBUG
	char logtemp[LOGSTRLEN];
	#endif

	s = platformstatus_GetStatus();
	
	pthread_mutex_lock( &rudderthrottle_mutex );
	r = global_rudder;
	t = global_throttle;
	pthread_mutex_unlock( &rudderthrottle_mutex );
	

	#ifdef PRINTSCREEN
	printf("\n");
	printf("controller_SetModeWaypoint:\n");
	printf("   wpt0 = (%f,%f,%5.2f,%5.2f)\n"
			,wp0.dLatDeg
			,wp0.dLonDeg
			,wp0.dCloseEnoughM
			,wp0.dSpeedMps);

	printf("   wpt1 = (%f,%f,%5.2f,%5.2f)\n"
			,wp1.dLatDeg
			,wp1.dLonDeg
			,wp1.dCloseEnoughM
			,wp1.dSpeedMps);
	#endif
	
	#ifdef DEBUG
	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   wpt0 = (%f,%f,%f,%f)"
			,wp0.dLatDeg
			,wp0.dLonDeg
			,wp0.dCloseEnoughM
			,wp0.dSpeedMps);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   wpt1 = (%f,%f,%f,%f)"
			,wp1.dLatDeg
			,wp1.dLonDeg
			,wp1.dCloseEnoughM
			,wp1.dSpeedMps);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
	#endif
	
	#ifdef PRINTSCREEN	
	printf("   position = (%f,%f)\n",s.dLatDeg,s.dLonDeg);
	printf("   time     = %d\n",s.nTime);
	
	printf("   groundspeed = %5.2f\n",s.dSpeed_mps);
	printf("   heading     = %5.1f\n",s.dHdgDeg);
	printf("   direction   = %5.1f\n",s.dDirectionDeg);

	printf("   oldrudder   = %d\n",r);
	printf("   oldthrottle = %d\n",t);
	#endif

	#ifdef DEBUG	
	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   position = (%f,%f)"
			,s.dLatDeg
			,s.dLonDeg);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   time     = %d"
			,s.nTime);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
	
	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   groundspeed = %f"
			,s.dSpeed_mps);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   heading     = %f"
			,s.dHdgDeg);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   direction   = %f"
			,s.dDirectionDeg);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   oldrudder   = %d"
			,r);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   oldthrottle = %d"
			,t);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
	#endif


	//***********************************
	//Do something useful here
	
	
	WAYPT temp;

	double desired_angle;
	double turn_angle, err;
	
	t = -100;

	temp.dLatDeg = s.dLatDeg ;
	temp.dLonDeg = s.dLonDeg;
	temp.dCloseEnoughM = 0;
	temp.dSpeedMps = 0;

	desired_angle = waypoints_HeadingFromWaypoint(temp,wp0);

	printf("\ndesired angle: %lf\n", desired_angle);
	turn_angle = (desired_angle - s.dHdgDeg);
	printf("\n turn angle: %lf \n",turn_angle);

	if (turn_angle >= 0){
		if (turn_angle <= 180)
			err = turn_angle;
		else
			err = -(360 - turn_angle);}
	else{
		if (turn_angle >= -180)
			err = turn_angle;
		else
			err = (360 + turn_angle);}
				
			
//*******************************
	/* PI Controller START */
	
	double kp = 1.0;		/* Propotional Gain for PID speed controller */
	double ki = 0;		/* Integral Gain for PID speed controller */
	double Ts = 0.1;		/* sampling time */

	static double PI_error = 0.0;

	if (PI_error < 10000)
		ki = 0.0;
	
	printf("test before: %lf \n", PI_error);

	PI_error += err;

	
	r = kp*err + ki*Ts*PI_error ;

	
	/* PI Controller END */
	//*******************************



	if (r > 100)
		r = 100;
	if (r < -100)
		r = -100;
	
	
	if (waypoints_DistanceFromWaypoint(temp,wp0) < wp0.dCloseEnoughM)
		t=0;

	printf("test after: %lf \n", PI_error);

	//r=0;
	
	//************************************


	#ifdef PRINTSCREEN
	printf("   newrudder   = %d\n",r);
	printf("   newthrottle = %d\n",t);
	#endif

	#ifdef DEBUG
	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   newrudder   = %d"
			,r);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);

	snprintf(logtemp,LOGSTRLEN
			,"controller_SetModeWaypoint:   newthrottle = %d"
			,t);
	logfile_LogLevelPrint(LOGLEVEL_VERBOSE,logtemp);
	#endif
	
	controls_SetRudder(r);
	controls_SetThrottle(t);

	pthread_mutex_lock( &rudderthrottle_mutex );
	global_rudder = r;
	global_throttle = t;
	pthread_mutex_unlock( &rudderthrottle_mutex );
	
	return;
}
